package com.quxiu.common.core.utils.map;

import com.alibaba.fastjson2.JSONObject;

import java.util.ArrayList;
import java.util.List;

import static java.lang.Math.*;

/**
 * 地图工具类
 */
public class GeoMapUtils {

    //WGS84坐标系，长轴6378137.000m，短轴6356752.314，扁率1/298.257223563。
    // 扁率
    public static final double f = 1 /  298.257223563;
    // 长轴
    public static final double majorAxis = 6378137.0;
    // 短轴
    public static final double minorAxis = 6356752.314245;

    /**
     * 计算两点间距离和方位角
     * @param lon1
     * @param lat1
     * @param lon2
     * @param lat2
     * @return
     */
    public static Line calculateLine(double lat1, double lon1, double lat2, double lon2) {
        GeoPoint a = new GeoPoint(lat1, lon1);
        GeoPoint b = new GeoPoint(lat2, lon2);

        return calculateLine(a, b);
    }
    /**
     * 计算两点间距离和方位角
     * @param a
     * @param b
     * @return
     */
    public static Line calculateLine(GeoPoint a, GeoPoint b) {
        //将经纬度坐标转换为弧度
        double lat1 = Math.toRadians(a.getLatitude());
        double lon1 = Math.toRadians(a.getLongitude());
        double lat2 = Math.toRadians(b.getLatitude());
        double lon2 = Math.toRadians(b.getLongitude());


        double L = lon2 - lon1;

        double tanU1 = (1 - f) * tan(lat1);
        double cosU1 = 1 / sqrt((1 + tanU1 * tanU1));
        double sinU1 = tanU1 * cosU1;

        double tanU2 = (1 - f) * tan(lat2);
        double cosU2 = 1 / sqrt((1 + tanU2 * tanU2));
        double sinU2 = tanU2 * cosU2;

        double lambda = L;
        double lambda_ = 0;
        double iterationLimit = 100;

        double cosSq_alpha = 0;

        double sin_delta = 0;
        double cos2_deltaM = 0;
        double cos_delta = 0;
        double delta = 0;
        double sinlambda = 0;
        double coslambda = 0;
        double s = 0;

        while (abs(lambda - lambda_) > 1e-12 && iterationLimit > 0) {
            iterationLimit = iterationLimit - 1;
            sinlambda = sin(lambda);
            coslambda = cos(lambda);
            double sinSq_delta = (cosU2 * sinlambda) * (cosU2 * sinlambda) + (cosU1 * sinU2 - sinU1 * cosU2 * coslambda) * (cosU1 * sinU2 - sinU1 * cosU2 * coslambda);
            sin_delta = sqrt(sinSq_delta);
            if (sin_delta == 0) {

            }
            cos_delta = sinU1 * sinU2 + cosU1 * cosU2 * coslambda;
            delta = atan2(sin_delta, cos_delta);
            double sin_alpha = cosU1 * cosU2 * sinlambda / sin_delta;
            cosSq_alpha = 1 - sin_alpha * sin_alpha;
            cos2_deltaM = cos_delta - 2 * sinU1 * sinU2 / cosSq_alpha;
            double C = f / 16 * cosSq_alpha * (4 + f * (4 - 3 * cosSq_alpha));
            lambda_ = lambda;
            lambda = L + (1 - C) * f * sin_alpha * (delta + C * sin_delta * (cos2_deltaM + C * cos_delta * (-1 + 2 * cos2_deltaM * cos2_deltaM)));
        }

        double uSq = cosSq_alpha * (majorAxis * majorAxis - minorAxis * minorAxis) / (minorAxis * minorAxis);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));
        double delta_delta = B * sin_delta * (cos2_deltaM + B / 4 * (cos_delta * (-1 + 2 * cos2_deltaM * cos2_deltaM) - B / 6 * cos2_deltaM * (-3 + 4 * sin_delta * sin_delta) * (-3 + 4 * cos2_deltaM * cos2_deltaM)));

        // 距离
        s = minorAxis * A * (delta - delta_delta);
        //初始方位角
        double fwdAz = atan2(cosU2 * sinlambda, cosU1 * sinU2 - sinU1 * cosU2 * coslambda);
        //最终方位角
        double revAz = atan2(cosU1 * sinlambda, -sinU1 * cosU2 + cosU1 * sinU2 * coslambda);

        double fwdAz_ = judge(Math.toDegrees(fwdAz));  //将角度归一化至0 - 360°内
        double revAz_ = judge(Math.toDegrees(revAz));  //将角度归一化至0 - 360°内

        Line line = new Line();
        line.setA(a);
        line.setB(b);
        line.setDist(s);
        line.setInitialAzimuth(fwdAz_);
        line.setFinalAzimuth(revAz_);
        return line;
    }

    /**
     * 将角度归一化至0 - 360°内
     * @param brng
     * @return
     */
    private static double judge ( double brng){
        if (brng < 0) {
            return brng + 360;
        } else {
            return brng;
        }
    }


    /**
     * 已知点A及点A到点B的距离dist和方位角度azimuth，求点B
     * @param lat
     * @param lon
     * @param azimuth
     * @param dist
     * @return
     */
    public static GeoPoint calculateThatPoint(double lat, double lon, double azimuth, double dist) {

        double alpha1 = rad(azimuth);
        double sinAlpha1 = Math.sin(alpha1);
        double cosAlpha1 = Math.cos(alpha1);

        double tanU1 = (1 - f) * Math.tan(rad(lat));
        double cosU1 = 1 / Math.sqrt((1 + tanU1 * tanU1));
        double sinU1 = tanU1 * cosU1;
        double sigma1 = Math.atan2(tanU1, cosAlpha1);
        double sinAlpha = cosU1 * sinAlpha1;
        double cosSqAlpha = 1 - sinAlpha * sinAlpha;
        double uSq = cosSqAlpha * (majorAxis * majorAxis - minorAxis * minorAxis) / (minorAxis * minorAxis);
        double A = 1 + uSq / 16384 * (4096 + uSq * (-768 + uSq * (320 - 175 * uSq)));
        double B = uSq / 1024 * (256 + uSq * (-128 + uSq * (74 - 47 * uSq)));

        double cos2SigmaM=0;
        double sinSigma=0;
        double cosSigma=0;
        double sigma = dist / (minorAxis * A), sigmaP = 2 * Math.PI;
        while (Math.abs(sigma - sigmaP) > 1e-12) {
            cos2SigmaM = Math.cos(2 * sigma1 + sigma);
            sinSigma = Math.sin(sigma);
            cosSigma = Math.cos(sigma);
            double deltaSigma = B * sinSigma * (cos2SigmaM + B / 4 * (cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)
                    - B / 6 * cos2SigmaM * (-3 + 4 * sinSigma * sinSigma) * (-3 + 4 * cos2SigmaM * cos2SigmaM)));
            sigmaP = sigma;
            sigma = dist / (minorAxis * A) + deltaSigma;
        }

        double tmp = sinU1 * sinSigma - cosU1 * cosSigma * cosAlpha1;
        double lat2 = Math.atan2(sinU1 * cosSigma + cosU1 * sinSigma * cosAlpha1,
                (1 - f) * Math.sqrt(sinAlpha * sinAlpha + tmp * tmp));
        double lambda = Math.atan2(sinSigma * sinAlpha1, cosU1 * cosSigma - sinU1 * sinSigma * cosAlpha1);
        double C = f / 16 * cosSqAlpha * (4 + f * (4 - 3 * cosSqAlpha));
        double L = lambda - (1 - C) * f * sinAlpha
                * (sigma + C * sinSigma * (cos2SigmaM + C * cosSigma * (-1 + 2 * cos2SigmaM * cos2SigmaM)));

        double revAz = Math.atan2(sinAlpha, -tmp); // final bearing

//        System.out.println(revAz);
//        System.out.println(lon+deg(L)+","+deg(lat2));
        double latB = deg(lat2);
        double lngB = lon+deg(L);
        GeoPoint result = new GeoPoint(latB, lngB);
        return result;
    }


    /**
     * 度换成弧度
     *
     * @param d
     *            度
     * @return 弧度
     */
    private static double rad(double d) {
        return d * Math.PI / 180.0;
    }

    /**
     * 弧度换成度
     *
     * @param x
     *            弧度
     * @return 度
     */
    private static double deg(double x) {
        return x * 180 / Math.PI;
    }

    /**
     * 已知WGS84坐标系 A 点，B点, X 在AB 弧线上, 且是最短的这条, AX距离已知，求X点坐标.
     * @param a
     * @param b
     * @param dist
     * @return
     */
    public static GeoPoint caculateRawGeoPoint(GeoPoint a, GeoPoint b, double dist){
        Line line = calculateLine(a, b);
        GeoPoint xPoint = calculateThatPoint(a.getLatitude(), a.getLongitude(), line.getInitialAzimuth(), dist);

        // 优化坐标
        xPoint = perpendicularFoot(xPoint.getLatitude(), xPoint.getLongitude(), a.getLatitude(), a.getLongitude(), b.getLatitude(), b.getLongitude());
        return xPoint;
    }

    /**
     * 获取点与线的垂点
     * @param lat
     * @param lon
     * @param lat1
     * @param lon1
     * @param lat2
     * @param lon2
     * @return
     */
    public static GeoPoint perpendicularFoot(double lat, double lon,
                                             double lat1, double lon1, double lat2, double lon2) {
        double[] p1 = {lat1, lon1};
        double[] p2 = {lat2, lon2};
        double[] p = {lat, lon};
        double[] v1 = Vector.sub(p, p1);
        double[] v2 = Vector.sub(p2, p1);
        double t = Vector.dot(v1, v2) / Vector.dot(v2, v2);
        double[] foot = Vector.add(p1, Vector.mult(v2, t));
        return new GeoPoint(foot[0], foot[1]);
    }

    public static void main(String[] args) {
        GeoPoint a = new GeoPoint(39.911825, 116.706702);
        GeoPoint b = new GeoPoint(39.278806, 117.282893);
        Line line = calculateLine(a, b);
        System.out.println(JSONObject.toJSONString(line));

//        GeoPoint result = caculateRawGeoPoint(a, b, 85959.22748641254);
//        System.out.println(JSONObject.toJSONString(result));
        int step = 2000;
        double dist = 0;
        List<JSONObject> list = new ArrayList<>();
        while (true) {
            GeoPoint result = caculateRawGeoPoint(a, b, dist);

            JSONObject item = new JSONObject();
            item.put("lat", result.getLatitude());
            item.put("lng", result.getLongitude());
            list.add(item);

            if(dist == line.getDist()) {
                break;
            } else if(dist + step < line.getDist()) {
                dist += step;
            } else {
                dist = line.getDist();
            }

        }
        System.out.println(JSONObject.toJSONString(list));
    }

}
